/* Copyright (c) 2006, Jan Flora <janflora@diku.dk>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *  - Neither the name of the University of Copenhagen nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
  @author Jan Flora <janflora@diku.dk>
*/

#include "mac.h"
#include "MacSuperframes.h"
#include "MacPib.h"

module BeaconTrackerM
{
	provides
	{
		interface IeeeRequest<Mlme_SyncRequest> as MlmeRequestSync;
		interface IeeeIndication<Mlme_BeaconNotifyIndication> as MlmeIndicationBeaconNotify;
		interface IeeeIndication<Mlme_SyncLossIndication> as MlmeIndicationSyncLoss;
		interface CapEvents as DeviceCap;
		#ifdef GTS_SUPPORT
			interface CapEvents as DeviceCfp;
		#endif
		interface Reset;
	}
	uses
	{
		interface MemoryManager as BufferMng;
		interface AsyncAlarm<time_t> as TrackAlarm;
		interface PhyAttributes;
		interface CapTx as DeviceTx;
		interface FrameRx;
		interface RxFrame as BeaconFrame;
		interface PanConflict;
		interface Superframe;
		interface PollService;
		interface MacAddress;
		interface CallbackService;
		interface MacFrame;
		#ifdef GTS_SUPPORT
			interface AsyncAlarm<time_t> as CfpAlarm;
			interface BeaconGtsService;
		#endif
		
		interface Debug;
	}
}
implementation
{
	#define DBG_LEVEL 1
	#include "Debug.h"

	void setBeaconTimeout();
	void disableBeaconMode();
	
	task void beaconNotify();
	task void poll();
	task void missedBeacon();
	task void syncLoss();
	task void processBeaconPayload();
	task void processGtsSpecs();
	task void processPendingAddrs();
	#ifdef GTS_SUPPORT
		task void processGtsSpecs();
	#endif
	
	// Beacon decoding structures.
	macMhr_t beaconHeader;
	macBeaconPayload_t beaconPayload;
	rxdata_t lastBeacon;
	
	bool pollPosted = FALSE;
	bool pollExt;

	bool listening = FALSE;
	norace bool tracking = FALSE;
	norace bool waitingForCapEnd = FALSE;
	norace bool waitingForCfpEnd = FALSE;
	norace uint8_t numBeaconsLost = 0;
	norace uint8_t channel;
	norace time_t nextCommence;
	norace time_t capEnd;
	norace time_t cfpEnd;

	txHeader_t myTxHeader;

	command void Reset.reset()
	{
		atomic {
			pollPosted = FALSE;
			listening = FALSE;
			tracking = FALSE;
			waitingForCapEnd = FALSE;
			waitingForCfpEnd = FALSE;
			numBeaconsLost = 0;
		}
	}

	command result_t MlmeRequestSync.request( Mlme_SyncRequest request )
	{
		// TODO: Make a queue so that we can have more than one sync request at
		//       a time.
		channel = request->msg.request.logicalChannel;
		tracking = request->msg.request.trackBeacon;
		
/*		if (channel > 26) {
			request->msg.confirm.status = IEEE802154_INVALID_PARAMETER;
			return FAIL;
		}
*/		
		// deallocate the primitive
		call BufferMng.release( sizeof(mlmeSyncRequestMsg_t), (uint8_t*)request );

		if (tracking) {
			// Enable beacon mode.
			atomic {
				macBeaconEnabled = TRUE;
				deviceCapActive = FALSE;
			}
			call PhyAttributes.setAckBackoffAlignment(TRUE);
			call PhyAttributes.setContentionWindow(2);
			
			atomic listening = TRUE;
			
			if (SUCCESS != call PhyAttributes.setChannel(channel)) {
				DBG_STRINT("WARNING: BeaconTracker, could not change radio channel!",channel,1);
			}
			if (PHY_SUCCESS != call FrameRx.rxOnNow()) {
				DBG_STR("FATAL: BeaconTracker, Unable to enable receiver",1);
			}
			setBeaconTimeout();
		}
		return SUCCESS;
	}

	async event uint8_t *BeaconFrame.received(rxdata_t *data)
	{
		uint8_t *newBuffer;
		bool wasListening;

		// stop the track alarm
		call TrackAlarm.stop();
		
		atomic {
			wasListening = listening;
			listening = FALSE;
		}
		if (!wasListening) { return data->frame; } // ignore stray beacons
		
		// Reset the lost beacon count.
		numBeaconsLost = 0;
		
		// Unpack frame structure.
		memcpy(&lastBeacon, data, sizeof(rxdata_t));
		call MacFrame.initializeMhrStruct(&beaconHeader, data->frame);
		call MacFrame.intializeBeaconStruct(&beaconPayload, beaconHeader.payload);

		// unpack superframe info and update deviceSuperframe accordingly
		call Superframe.updateFromSpec( &deviceSuperframe,
		                                //msduGetSuperframeSpec(data->frame),
		                                beaconPayload.sfSpec,
		                                data->recvTime,
		                                data->length);
		
		// Set up the track alarm to fire just before next beacon
		nextCommence = call Superframe.getNextStart(&deviceSuperframe);
		call TrackAlarm.armAlarmClock(nextCommence-50);

		#ifdef GTS_SUPPORT
			// We end the cap 82 symbols before real cap end.
			// This is due to the fact, that the shortest tx packet takes
			// 30 symbols to transmit. Including both 2*CCA + SIFS makes 82 symbols.
			capEnd = call Superframe.getCapEnd(&deviceSuperframe);
			if (capEnd < nextCommence) {
				// We need to activate CFP or idle period at CAP end.
				call CfpAlarm.armAlarmClock(capEnd-82);
				waitingForCapEnd = TRUE;
			}
		#endif
		
		atomic deviceCapActive = TRUE;
		signal DeviceCap.startNotification();
		//offset = data->recvTime + (8 + data->length)*2;
		//DBG_STRINT("CAP begining is:", offset,1);
		
		// The rest of the beacon is processed in tasks.
		#ifdef GTS_SUPPORT
			post processGtsSpecs();
		#endif
		
		if (!macAutoRequest) {
			post beaconNotify();
		} else {
			post processPendingAddrs();
		}

		//DBG_STRINT("Cap end is:",capEnd,1);
		//DBG_STRINT("Next commence is:",nextCommence,1);
		
		// DEBUG: We need to be able to check, that all beacon frames are heard!
		//DBG_STRINT("Seqence number:",mhrSeqNumber(data->frame),1);
		
		// Allocate a new receive buffer.
		if (SUCCESS != call BufferMng.claim(126, &newBuffer)) {
			DBG_STR("FATAL: BeaconTracker, could not claim memory for new receive buffer",1);
		}
		return newBuffer;
	}

	task void beaconNotify()
	{
		mlmeBeaconNotifyIndication_t *beaconNotifyInd;
		// create a beacon notify primitive
		if (SUCCESS != call BufferMng.claim(sizeof(mlmeBeaconNotifyIndication_t),(uint8_t**)(&beaconNotifyInd))){
			DBG_STR("WARNING: BeaconTracker, Unable to claim buffer for beacon notification!",1);
			// Deallocate beacon frame.
			call BufferMng.release(126, lastBeacon.frame);
			return;
		}
		beaconNotifyInd->destroyable = TRUE;
		
		// If beacon is < 64 bytes we copy content into a smaller buffer to save space.
		if (lastBeacon.length < 64) {
			uint8_t *myBeacon;
			if (SUCCESS != call BufferMng.claim(lastBeacon.length, &myBeacon)) {
				DBG_STR("FATAL: BeaconTracker, could not claim memory for beacon",1);
				// Deallocate beacon frame.
				call BufferMng.release(126, lastBeacon.frame);
				return;
			}
			memcpy(myBeacon, lastBeacon.frame, lastBeacon.length);
			beaconNotifyInd->msg.indication.beaconFrame = myBeacon;
			beaconNotifyInd->msg.indication.frameLength = lastBeacon.length;
			// We need to re-decode the new beacon frame to get the correct pointers.
			call MacFrame.initializeMhrStruct(&beaconHeader, myBeacon);
			call MacFrame.intializeBeaconStruct(&beaconPayload, beaconHeader.payload);
			// Deallocate beacon frame.
			call BufferMng.release(126, lastBeacon.frame);
		} else {
			beaconNotifyInd->msg.indication.beaconFrame = lastBeacon.frame;
			beaconNotifyInd->msg.indication.frameLength = 126;
		}
		beaconNotifyInd->msg.indication.sfSpec = beaconPayload.sfSpec;
		beaconNotifyInd->msg.indication.gtsPermit = beaconPayload.gtsSpec->GTSPermit;
		beaconNotifyInd->msg.indication.addrSpec = beaconPayload.addrSpec;
		beaconNotifyInd->msg.indication.addrList = beaconPayload.addrList;
		beaconNotifyInd->msg.indication.payload = beaconPayload.payload;
		beaconNotifyInd->msg.indication.payloadLength = lastBeacon.length - (call MacFrame.getMhrSize(&beaconHeader)+call MacFrame.getBeaconSize(&beaconPayload));
		beaconNotifyInd->msg.indication.logicalChannel = channel;
		beaconNotifyInd->msg.indication.linkQuality = lastBeacon.linkQuality;
		beaconNotifyInd->msg.indication.timeStamp = lastBeacon.recvTime;
		beaconNotifyInd->msg.indication.ACLEntry = 0x08; // TODO
		beaconNotifyInd->msg.indication.securityFailure = FALSE; // TODO
		// Notify upper layer with beacon
		signal MlmeIndicationBeaconNotify.indication(beaconNotifyInd);
	}

	async event void PollService.done(Ieee_Status status)
	{
		// Do nothing here. If we fail, we just retry next superframe.
	}

	#ifdef GTS_SUPPORT
		async event result_t CfpAlarm.alarm()
		{
			if (waitingForCapEnd) {
				// CAP has ended.
				deviceCapActive = FALSE;
				#ifdef GTS_SUPPORT
					if (call Superframe.cfpExists(&deviceSuperframe)) {	
						// We start up the device CFP.
						signal DeviceCfp.startNotification();
						cfpEnd = call Superframe.getCfpEnd(&deviceSuperframe);
						if (cfpEnd < nextCommence) {
							call CfpAlarm.armAlarmClock(cfpEnd-82);
							waitingForCfpEnd = TRUE;
						}
					} else {
						// TODO: Below note could be a problem.
						// NOTE: Coordinator CAP and CFP can be active in the idle period.
						// Idle period.
					}
				#endif
				waitingForCapEnd = FALSE;
			} else if (waitingForCfpEnd) {
				// TODO: Below note could be a problem.
				// NOTE: Coordinator CAP and CFP can be active in the idle period.
				// Idle period.
				waitingForCfpEnd = FALSE;
			}
			return SUCCESS;
		}
	#endif
	
	async event result_t TrackAlarm.alarm()
	{
		bool wasListening;
		DBG_STR("TrackAlarm",2);
		atomic {
			wasListening = listening;
			listening = FALSE;
		}
		if (wasListening) {
			// this is a timeout, beacon missed
			DBG_STR("BeaconTracker, beacon timeout!",1);
			post missedBeacon();
		} else {
			if (tracking) {
				// Turn off the transceiver.
				call FrameRx.trxOff(TRUE);
				// we need to start listening
				if (PHY_SUCCESS != call FrameRx.rxOn(nextCommence-10)) {
					DBG_STR("Warning: BeaconTracker, could not enable receiver!",1);
				}
				// Disable all CAPs
				atomic {
					deviceCapActive = FALSE;
					listening = TRUE;
				}
				setBeaconTimeout();
			} else {
				// Disable beacon mode.
				disableBeaconMode();
			}
		}
		return SUCCESS;
	}
	
	async event void PanConflict.conflictDetected()
	{
		// A PAN conflict was detected.
		// Report to coordinator.
		uint8_t *conflictNotification;
		macMhr_t myMhr;
		ieeeAddress_t srcAddr, dstAddr;
		// Allocate a buffer big enough for 2*extended addressing.
		if (SUCCESS != call BufferMng.claim(24, &conflictNotification)) {
			DBG_STR("FATAL: BeaconTracker, Unable to claim new receive buffer!",1);
		}
		
		call MacFrame.initializeMhrStruct(&myMhr, conflictNotification);
		
		myMhr.frameControl->AckRequest = TRUE;
		myMhr.frameControl->SecurityEnabled = FALSE;
		myMhr.frameControl->FramePending = FALSE;
		myMhr.frameControl->FrameType = macCommandFrame;
		call MacAddress.getCoordinatorAddr(&dstAddr);
		call MacAddress.getLocalAddr(&srcAddr, FALSE);
		call MacAddress.setAddrs(&myMhr, &srcAddr, &dstAddr);
		((macCommandPayload_t*)myMhr.payload)->commandFrameIdent = macCommandPanIdConf;
		
/*		mhrAckRequest(conflictNotification) = TRUE;
		mhrSecurityEnabled(conflictNotification) = FALSE;
		mhrFramePending(conflictNotification) = FALSE;
		call MacAddress.setDstCoordinator(conflictNotification);
		call MacAddress.setSrcLocal(conflictNotification, FALSE);
		
		mhrFrameType(conflictNotification) = macCommandFrame;
		msduCommandFrameIdent(conflictNotification) = macCommandPanIdConf;*/
		
		// Send the frame in the CAP, though not explicitly stated in the standard.
		// Build the txHeader.
		myTxHeader.addDsn = TRUE;
		myTxHeader.frame = conflictNotification;
		myTxHeader.length = call MacFrame.getMhrSize(&myMhr) + 1;
		//myTxHeader.length = mhrLengthFrame(conflictNotification) + 1;
		myTxHeader.isData = FALSE;
		
		call DeviceTx.sendFrame(&myTxHeader);
	}
	
	event void DeviceTx.done(txHeader_t *header)
	{
		// Deallocate the conflict notification frame!
		call BufferMng.release(24, header->frame);
	
		// We silently ignore transmission failures.
		if (tracking && myTxHeader.status == IEEE802154_SUCCESS) {
			// Indicate sync loss and disable beacon mode.
			disableBeaconMode();
			post syncLoss();
		}
	}
	
	task void syncLoss()
	{
		mlmeSyncLossIndication_t *syncLossInd;
		if (SUCCESS != call BufferMng.claim(sizeof(mlmeSyncLossIndication_t),(uint8_t**)(&syncLossInd))) {
			DBG_STR("FATAL: BeaconTracker, Unable to claim buffer for sync loss indication!",1);
		} else {
			syncLossInd->msg.indication.lossReason = IEEE802154_BEACON_LOSS;
			signal MlmeIndicationSyncLoss.indication(syncLossInd);
		}
	}

	task void poll()
	{
		bool doExtPoll;
		atomic {
			pollPosted = FALSE;
			doExtPoll = pollExt;
		}
		call PollService.pollCoordinator(doExtPoll);
		
	}

	void disableBeaconMode()
	{
		atomic {
			macBeaconEnabled = FALSE;
			deviceCapActive = TRUE;
		}
		call PhyAttributes.setAckBackoffAlignment(FALSE);
		call PhyAttributes.setContentionWindow(1);
	}

	void setBeaconTimeout()
	{
		time_t timeout;
		// listen for aBaseSuperframeDuration * (1<<macBeaconOrder+1) symbols
		DBG_STR("listen",2);
		timeout = 1;
		timeout = ((timeout << macBeaconOrder)+1)*aBaseSuperframeDuration;

		// set up the alarm for track timeout
		if ( SUCCESS != call TrackAlarm.armCountdown(timeout) ){
			DBG_STR("FATAL: BeaconTracker, Unable to arm timeout alarm",1);
		}
		DBG_STR("listening",2);
	}
	
	task void missedBeacon()
	{
		DBG_STR("Missed a beacon",1);

		// Increase loss count, and see if we lost sync or we have to retry.
		numBeaconsLost++;
		if (numBeaconsLost == aMaxLostBeacons) {
			// Sync was lost.
			// Disable beacon mode.
			disableBeaconMode();
			post syncLoss();
		} else {
			// Receiver already enabled. Start listening again.
			atomic {
				listening = TRUE;
			}
			setBeaconTimeout();
		}
	}
	
	#ifdef GTS_SUPPORT
		task void processGtsSpecs()
		{
			uint8_t numGtsDesc;
		
			// Check GTS fields.
			numGtsDesc = beaconPayload.gtsSpec->GTSDescriptorCount;
			if (numGtsDesc) {
				// Check if there are some changes for this device.
				uint8_t i;
				//uint8_t gtsDirections = msduGTSDirectionMask(data->frame);
				uint8_t gtsDirections = beaconPayload.gtsDirs->GTSDirectionMask;
				//msduGTSList_t *gtsList = msduGTSList(data->frame);
				msduGTSList_t *gtsList = beaconPayload.gtsList;
	
				for (i=0;i<numGtsDesc;i++) {
					if (gtsList->DeviceShortAddress == macShortAddress) {
						// We have a GTS update.
						call BeaconGtsService.gtsUpdate(gtsList->GTSStartingSlot, gtsList->GTSLength, gtsDirections & (1<<i));
					}
					gtsList += sizeof(msduGTSList_t);
				}
			}
			call BeaconGtsService.beaconReceived();
		}
	#endif
	
	task void test()
	{
		call BufferMng.release(126, lastBeacon.frame);
	}
	
	task void processPendingAddrs()
	{		
		// Check if we have pending data.
		uint8_t i;
		uint8_t numShortAddrs = beaconPayload.addrSpec->NumShortAddrsPending;
		uint8_t numExtAddrs = beaconPayload.addrSpec->NumExtAddrsPending;
		uint8_t *addrList = beaconPayload.addrList;
		//DBG_DUMP(data->frame, data->length, 1);

		// Check short addresses.
		for (i=0;i<numShortAddrs;i++) {
			atomic {
				if (*((uint16_t*)addrList) == macShortAddress) {
					if (!pollPosted) {
						pollPosted = post poll();
						pollExt = FALSE;
					}
					DBG_STR("Pending for me, Short",1);
				}
			}
			//DBG_STR("Short addr pending:",1);
			//DBG_DUMP(addrList,2,1);
			addrList += 2;
		}
		// Check extended addresses.
		for (i=0;i<numExtAddrs;i++) {
			bool match = TRUE;
			uint8_t j;
			for (j=0;j<8;j++) {
				if (addrList[j] != aExtendedAddress[j]) {
					match = FALSE;
					break;
				}
			}
			if (match) {
				atomic {
					if (!pollPosted) {
						pollPosted = post poll();
						pollExt = TRUE;
					}
				}
				DBG_STR("Pending for me, Extended",1);
			}
			//DBG_STR("Extended addr pending:",1);
			//DBG_DUMP(addrList,8,1);
			addrList += 8;
		}
		
		// Check if we have beacon payload.
		if (addrList < (lastBeacon.frame + lastBeacon.length)) {
			DBG_STR("We have beacon payload!",1);
			post beaconNotify();
		} else {
			// Deallocate beacon frame.
			call BufferMng.release(126, lastBeacon.frame);
		}
	}
	
	default event void MlmeIndicationSyncLoss.indication( Mlme_SyncLossIndication indication )
	{
		// Ignore the indication and deallocate the primitive!
		call BufferMng.release(sizeof(mlmeSyncLossIndication_t), (uint8_t*)indication);
		DBG_STR("WARNING: Sync loss indication was not handled!",1);
	}
	
	default event void MlmeIndicationBeaconNotify.indication( Mlme_BeaconNotifyIndication indication )
	{
		// Ignore the indication and deallocate the primitive!
		call BufferMng.release(indication->msg.indication.frameLength, indication->msg.indication.beaconFrame);
		call BufferMng.release(sizeof(mlmeBeaconNotifyIndication_t),(uint8_t*)indication);
		DBG_STR("WARNING: Beacon notify indication was not handled!",1);
	}
}
